/*!
  @file detector_server_test.cc
  @brief Brief description
  @author Kyohei Otsu <kyohei@kth.se>
  @date 2013
*/

#include <ros/ros.h>

#include "mmo_vo/DetectorInquiry.h"

using namespace mmo_vo;

namespace vo
{

//! class description 
/*! This is a detailed description of the class */
class DetectorServerTest {
 private:
  //! ROS node handler
  ros::NodeHandle nh_;
  //! ROS node handler in the private namespace
  ros::NodeHandle private_nh_;

 protected:

 public:
  //! Constructor
  DetectorServerTest(ros::NodeHandle &nh) : nh_(nh), private_nh_("~")
  {
    // initializing pub/sub, server

    // initializing optional parameters
    // e.g.) private_nh_.param<TYPE>("PARAM_NAME", PARAM_VAR, DEFAULT_VALUE);
  }

  //! Destructor
  ~DetectorServerTest()
  {
  }

  void request();
  void update(int id);
};

void DetectorServerTest::request()
{
  // ask the current algorithm
  ros::ServiceClient detector_client = nh_.serviceClient<mmo_vo::DetectorInquiry>("detector_inquiry");
  mmo_vo::DetectorInquiry inquiry;
  inquiry.request.id = inquiry.request.INQUIRY;
  if(detector_client.call(inquiry))
  {
    ROS_INFO_STREAM("Current crient: " << inquiry.response.id);
  }
  else
  {
    ROS_ERROR("Detector inquiry failed");
  }
}

void DetectorServerTest::update(int id)
{
  // update the current algorithm
  ros::ServiceClient detector_client = nh_.serviceClient<DetectorInquiry>("detector_inquiry");
  DetectorInquiry inquiry;
  inquiry.request.id = id;
  if(detector_client.call(inquiry))
  {
    ROS_INFO_STREAM("Current crient: " << inquiry.response.id);
  }
  else
  {
    ROS_ERROR("Detector inquiry failed");
  }
}

} // end of namespace

int main(int ac, char **av)
{
  ros::init(ac, av, "DetectorServerTest");
  ros::NodeHandle nh;
  vo::DetectorServerTest handler(nh);
  handler.request();
  handler.update(DetectorInquiry::Response::FEATURE_FAST);
  handler.request();
  ros::spin();
  return 0;
}

